import os
from launch import LaunchDescription
from launch.substitutions import LaunchConfiguration
from launch.actions import DeclareLaunchArgument,IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory



def generate_launch_description():
    # 获取目录
    robot_navigation2_dir = get_package_share_directory(
        'robot_navigation2')
    nav2_params_dir = get_package_share_directory('nav2_bringup')
    rviz_config_dir = os.path.join(
        nav2_params_dir,'rviz','nav2_default_view.rviz')


    # launch配置
    use_sim_time = LaunchConfiguration(
        'use_sim_time',default='True')
    map_yaml_path = LaunchConfiguration(
        'map',default=os.path.join(robot_navigation2_dir,'maps','room.yaml'))
    nav2_param_path = LaunchConfiguration(
        'params_file',default=os.path.join(robot_navigation2_dir,'config','nav2_params.yaml'))
    
    return LaunchDescription([
        # 声明新的launch参数
        DeclareLaunchArgument('use_sim_time',default_value=use_sim_time),
        DeclareLaunchArgument('map',default_value=map_yaml_path),
        DeclareLaunchArgument('params_file',default_value=nav2_param_path),

        IncludeLaunchDescription(
            PythonLaunchDescriptionSource(os.path.join(nav2_params_dir, 'launch', 'bringup_launch.py')),
            # 使用launch参数替换原有参数
            launch_arguments={
                'use_sim_time':use_sim_time,
                'map':map_yaml_path,
                'params_file':nav2_param_path}.items()
        ),

        Node(
            package='rviz2',
            executable='rviz2',
            arguments=['-d',rviz_config_dir],
            parameters=[{'use_sim_time':use_sim_time}],
            output='screen'
        )


    ])